import cv2
import numpy as np
import copy
import math
import sys
sys.path.append('././')
from kinematic import *
# 全局变量，用于存储 HSV 范围值
hsv_lower = np.array([0, 0, 0])
hsv_upper = np.array([179, 255, 255])
#标定结果
RT_cam_to_end = np.load('calibration/handeye_clib/eye_to_end.npy')
# 相机内参矩阵和畸变系数
cam_mtx = np.array( [[670.92263044  ,      0.        ,   358.82135372],
                    [  0.           ,    673.8728403 ,   224.19994634],
                    [  0.           ,      0.        ,     1.        ]], dtype=np.float64)

cam_dist = np.array([-0.19770613, -0.19343703 ,-0.00058339, -0.00063087 , 0.07889203], dtype=np.float64)

# 回调函数，用于调整 HSV 范围值
def on_trackbar_hue_min(value):
    global hsv_lower
    hsv_lower[0] = value
    # extract_roi()

def on_trackbar_hue_max(value):
    global hsv_upper
    hsv_upper[0] = value
    # extract_roi()

def on_trackbar_saturation_min(value):
    global hsv_lower
    hsv_lower[1] = value
    # extract_roi()

def on_trackbar_saturation_max(value):
    global hsv_upper
    hsv_upper[1] = value
    # extract_roi()

def on_trackbar_value_min(value):
    global hsv_lower
    hsv_lower[2] = value
    # extract_roi()

def on_trackbar_value_max(value):
    global hsv_upper
    hsv_upper[2] = value
    # extract_roi()

# 检测轮廓是否为矩形 contour 轮廓信息
def get_rectangle_infor(contour,target_size):
    """检测矩形并获取目标img_pose

    Args:
        contour (_list_): _description_
        target_size (_tuple_): (长，宽)

    Returns:
        _type_: _description_
    """
    rect_flag = 0 # 检测轮廓是否为矩形

    # 凸包(Convex Hull)
    hull_contour = cv2.convexHull(contour)
    contours = hull_contour

    rect = cv2.minAreaRect(contours)  # 检测轮廓最小外接矩形，得到最小外接矩形的（中心(x,y), (宽,高), 旋转角度）
    
    #轮廓约束检查
    area = int(rect[1][0]*rect[1][1])  # 计算包围形状的面积
    if area < 1200 or area>(320*240):  # 过滤面积过小或过大的形状
        rect_flag = 0
        return rect_flag, 0, 0
    long=0
    short=0
    if rect[1][0]>rect[1][1]:
        long=rect[1][0]
        short=rect[1][1]
    else:
        long=rect[1][1]
        short=rect[1][0]
    ratio=target_size[0]/target_size[1]#长比宽
    if long/short>ratio*1.3 or long/short<ratio*0.7:
        rect_flag = 0
        return rect_flag, 0, 0

    #获取imgpose
    box = cv2.boxPoints(rect)  # 获取最小外接矩形的4个顶点坐标
    # box_temp = np.copy(box)
    box_temp = np.zeros((5,2), np.float32)
    box_temp[2] = rect[0] # 矩形中心坐标
    
    #计算象素差
    delta_x=[]
    delta_y=[]
    for i in range(4):
        delta_x.append(box[i][0]-box_temp[2][0])
        delta_y.append(box_temp[2][1]-box[i][1])
    #计算角度
    angles=[]#存的是角度及对应的在box中的索引
    for i in range(4):
        angles.append([math.atan2(delta_y[i],delta_x[i])*180/math.pi,i])
    #按角度升序排序
    angles.sort(key=lambda x:x[0])
    #比较
    index=[-1,-1,-1,-1]
    if abs(angles[1][0]-0)<1e-6 and abs(angles[3][0]-180)<1e-6 and angles[2][0]<90:
        index[0]=angles[2][1]
        index[1]=angles[1][1]
        index[2]=angles[3][1]
        index[3]=angles[0][1]
    else:
        index[0]=angles[3][1]
        index[1]=angles[2][1]
        index[2]=angles[0][1]
        index[3]=angles[1][1]
    
    if all(index[i]==-1 for i in range(4)):
        rect_flag = 0
    else:
        box_temp[0]=box[index[0]]
        box_temp[1]=box[index[1]]
        box_temp[3]=box[index[2]]
        box_temp[4]=box[index[3]]
        rect_flag = 1
    return rect_flag, box_temp, rect

# 计算世界坐标的排列顺序
def get_target_world_pose(target_img_pose, target_size):
    target_word_pose = np.zeros((5,3), np.float32)
    print("target_img_pose",target_img_pose)

    t_h = np.linalg.norm(target_img_pose[1] - target_img_pose[0]) # 两个顶点的距离
    t_w = np.linalg.norm(target_img_pose[3] - target_img_pose[0])
    print(t_h, t_w)
    if t_h >= t_w:
        if target_size[0] >= target_size[1]:
            world_x = target_size[0]
            world_y = target_size[1]
        else:
            world_x = target_size[1]
            world_y = target_size[0]  
    else: 
        if target_size[0] <= target_size[1]:
            world_x = target_size[0]
            world_y = target_size[1]
        else:
            world_x = target_size[1]
            world_y = target_size[0]  
    #注意，按这样建系，世界坐标系的z轴朝下
    target_word_pose[0] = [-world_x/2, -world_y/2, 0]
    target_word_pose[1] = [world_x/2, -world_y/2, 0]
    target_word_pose[2] = [0,0, 0]
    target_word_pose[3] = [-world_x/2, world_y/2, 0]
    target_word_pose[4] = [world_x/2, world_y/2, 0]
    
    return target_word_pose

def get_target_base_pose(target_img_pose,target_world_pose, target_size,RT_end_to_base):
    retval, rvec, tvec = cv2.solvePnP(target_world_pose, target_img_pose, cam_mtx, cam_dist)
    if retval:
        RT = np.column_stack(((cv2.Rodrigues(rvec))[0],tvec))
        RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
        RT_target_to_cam = RT
        RT_target_to_base=RT_end_to_base@RT_cam_to_end@RT_target_to_cam
        return True,RT_target_to_base
    else:
        return False,None
    

# 回调函数，用于提取符合 HSV 范围的区域并显示结果
def extract_roi(frame):
    # 将图像转换为 HSV 格式
    hsv_image = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    # 根据调整的 HSV 范围提取区域
    mask = cv2.inRange(hsv_image, hsv_lower, hsv_upper)
    result = cv2.bitwise_and(frame, frame, mask=mask)

    temp=copy.deepcopy(frame)
    #下面是测试角点检测算法
    
    # # Harris 角点检测参数设置
    # block_size = 2  # 角点检测中指定区域大小
    # ksize = 3  # Sobel 梯度计算核大小
    # k = 0.04  # Harris 角点检测自由参数，取值范围[0.04, 0.06]

    # # 使用 Harris 角点检测算法
    # dst = cv2.cornerHarris(mask, block_size, ksize, k)

    # # 膨胀角点以便于标识
    # dst = cv2.dilate(dst, None)

    # # 标记角点
    # temp[dst > 0.01 * dst.max()] = [0, 0, 255]  # 在检测到的角点处绘制红色标记

    
    
    
    
    
    # binary=cv2.threshold(blurred,127,255,cv2.THRESH_OTSU)[1]
    # cv2.imshow('binary',binary)
    
    (cnts, _) = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)  # 轮廓检测
    target_img_pose = []
    target_world_pose = []
    shape_infor = []
    detect_flag = 0 #是否存在指定形状的flag
    for i, contour in enumerate(cnts):
        
        retval, target_img_pose, shape_infor = get_rectangle_infor(contour,target_size=(32,12))
        
        #检测轮廓成功
        if retval == 1:
            detect_flag = 1 
            target_world_pose=get_target_world_pose(target_img_pose,(32,12))#target_size根据实际改
            print(target_world_pose)
            break#这里每诊只检测一个
            # cv2.rectangle(image,rec=infor,color=(0,255,0),thickness=2)
            # cv2.imshow('image',image)
            # cv2.waitKey(0)
    if detect_flag:
        print("检测到目标形状")
        
        box = cv2.boxPoints(shape_infor)
        box = np.int0(box)
        # 在原图上绘制最小外包矩形
        cv2.drawContours(temp, [box], 0, (0, 255, 0), 2)
   
        cv2.arrowedLine(temp,(int(target_img_pose[2][0]), int(target_img_pose[2][1])),(int((target_img_pose[1][0]+target_img_pose[4][0])/2), int((target_img_pose[1][1]+target_img_pose[4][1])/2)),color=(0,255,0))
        cv2.arrowedLine(temp,(int(target_img_pose[2][0]), int(target_img_pose[2][1])),(int((target_img_pose[3][0]+target_img_pose[4][0])/2), int((target_img_pose[3][1]+target_img_pose[4][1])/2)),color=(255,0,0))
        cv2.circle(temp,(int(target_img_pose[2][0]), int(target_img_pose[2][1])), 3, (0, 0, 255),-1) 
        cv2.putText(temp, "1", (int(target_img_pose[0][0]), int(target_img_pose[0][1])), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 255, 0), 2)
        cv2.putText(temp, "2", (int(target_img_pose[1][0]), int(target_img_pose[1][1])), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 255, 0), 2)
        cv2.putText(temp, "3", (int(target_img_pose[3][0]), int(target_img_pose[3][1])), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 255, 0), 2)
        cv2.putText(temp, "4", (int(target_img_pose[4][0]), int(target_img_pose[4][1])), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 255, 0), 2)
        # cv2.rectangle(image,rec=infor,color=(0,255,0),thickness=2)
        
        RT_end_to_base=forward_kinematic(np.deg2rad([0,102,-48,-108,0]))
        retval,RT_target_to_base=get_target_base_pose(target_img_pose,target_world_pose,target_size=(32,12),RT_end_to_base=RT_end_to_base)
        if retval:
            RT_target_to_base=np.array(RT_target_to_base)
            cv2.putText(temp, "targetTobase:"+"{:.2f}".format(RT_target_to_base[0,3])+','+"{:.2f}".format(RT_target_to_base[1,3])+','+"{:.2f}".format(RT_target_to_base[2,3]), (10, 100), cv2.FONT_HERSHEY_COMPLEX, 0.5, (0, 255, 0), 1)
            #算 长边偏角
            angle=0
            if np.linalg.norm(target_img_pose[1] - target_img_pose[0])>np.linalg.norm(target_img_pose[3] - target_img_pose[0])  :
                #说明世界坐标系的x轴与长边平行
                angle=math.atan2(RT_target_to_base[1,0],RT_target_to_base[0,0])
            else:
                angle=math.atan2(RT_target_to_base[1,1],RT_target_to_base[0,1])
            cv2.putText(temp, "angle:"+"{:.2f}".format(angle*180/math.pi), (10, 150), cv2.FONT_HERSHEY_COMPLEX, 0.5, (0, 255, 0), 1)
                
    cv2.imshow('目标检测结果',temp)
    cv2.waitKey(1)
    # 显示结果
    cv2.imshow("mask",mask)
    # cv2.imshow("Original Image", frame)
    cv2.imshow("Extracted ROI", result)


# 创建一个窗口
cv2.namedWindow("Original Image")
cv2.namedWindow("Extracted ROI")

# 创建滑动条用于调整 HSV 范围值
cv2.createTrackbar("Hue Min", "Extracted ROI", hsv_lower[0], 179, on_trackbar_hue_min)
cv2.createTrackbar("Hue Max", "Extracted ROI", hsv_upper[0], 179, on_trackbar_hue_max)
cv2.createTrackbar("Saturation Min", "Extracted ROI", hsv_lower[1], 255, on_trackbar_saturation_min)
cv2.createTrackbar("Saturation Max", "Extracted ROI", hsv_upper[1], 255, on_trackbar_saturation_max)
cv2.createTrackbar("Value Min", "Extracted ROI", hsv_lower[2], 255, on_trackbar_value_min)
cv2.createTrackbar("Value Max", "Extracted ROI", hsv_upper[2], 255, on_trackbar_value_max)

# 初始化显示
# extract_roi()
cap = cv2.VideoCapture(0)

# 遍历视频帧
while cap.isOpened():
    # 从视频中读取一帧
    success, frame = cap.read()

    if success:
        extract_roi(frame)
        
    key = cv2.waitKey(1) 
    if key == 27:
        break



# 关闭窗口
cv2.destroyAllWindows()
